#include "driver_chassis.h"
#include "driver_gimbal.h"
#include "driver_superc.h"
#include "driver_remote.h"
#include "driver_communicate.h"

extern receive_judge_t judge_rece_mesg;
extern Super_Cap_t superCap;
extern int32_t LostCounterCountNumber[NUMBERS_OF_COUNT];
extern judgement_protection_struct judgement_protection;

float speed[5];
float setspeed[5];


ChassisFollow_TypeDef ChassisFollow;
ChassisMotorStruct  ChassisMotor[4];
GimbalMotorStruct   YawMotor;
ChassisSpeedMessegePort ChassisSpeed; //底盘速度
ChassisPowerDataStruct Power;
ChassisSpeedDataStruct SpeedLimit;
RoboStatu_Typedef Status;
PID ChassisSpeedPIDControl;

void ChassisInit(void)
{
    for(int i = 0; i < 4; i++)
    {
        ChassisMotor[i].PIDSpeed.Kp = CHASSIS_MOTOR_KP;
        ChassisMotor[i].PIDSpeed.Ki = CHASSIS_MOTOR_KI;
        ChassisMotor[i].PIDSpeed.Kd = CHASSIS_MOTOR_KD;
        ChassisMotor[i].PIDSpeed.calc = &PidCalc;
        ChassisMotor[i].PIDSpeed.clear = &PidClear;
        ChassisMotor[i].PIDSpeed.OutMax = 1;
        ChassisMotor[i].PIDSpeed.OutMin = -1;
        ChassisMotor[i].PIDSpeed.clear(&ChassisMotor[i].PIDSpeed);
        ChassisMotor[i].PIDSpeed.calc2 = &PidCalc2;
        ChassisMotor[i].PIDSpeed.Ap = CHASSIS_MOTOR_AP;
        ChassisMotor[i].PIDSpeed.Bp = CHASSIS_MOTOR_BP;
        ChassisMotor[i].PIDSpeed.Cp = CHASSIS_MOTOR_CP;
        ChassisMotor[i].PIDSpeed.Ai = CHASSIS_MOTOR_AI;
        ChassisMotor[i].PIDSpeed.Ci = CHASSIS_MOTOR_CI;
        ChassisMotor[i].PIDSpeed.Ad = CHASSIS_MOTOR_AD;
        ChassisMotor[i].PIDSpeed.Bd = CHASSIS_MOTOR_BD;
        ChassisMotor[i].PIDSpeed.Cd = CHASSIS_MOTOR_CD;
        ChassisMotor[i].PIDSpeed.ErrRef = 0.0f;		//控制积分分离变量
    }
    ChassisFollow.PIDChassisFollow.Kp	=	CHASSIS_FOLLOW_KP;
    ChassisFollow.PIDChassisFollow.Ki	=	CHASSIS_FOLLOW_KI;
    ChassisFollow.PIDChassisFollow.Kd	=	CHASSIS_FOLLOW_KD;

    ChassisFollow.PIDChassisFollow.OutMax	=	1;
    ChassisFollow.PIDChassisFollow.OutMin	=	-1;
    ChassisFollow.PIDChassisFollow.calc = &PidCalc;
    ChassisFollow.PIDChassisFollow.clear = &PidClear;
    ChassisFollow.PIDChassisFollow.calc2 = &PidCalc2;
    ChassisFollow.PIDChassisFollow.Ap = CHASSIS_FOLLOW_AP;
    ChassisFollow.PIDChassisFollow.Bp = CHASSIS_FOLLOW_BP;
    ChassisFollow.PIDChassisFollow.Cp = CHASSIS_FOLLOW_CP;
    ChassisFollow.PIDChassisFollow.Ai = CHASSIS_FOLLOW_AI;
    ChassisFollow.PIDChassisFollow.Ci = CHASSIS_FOLLOW_CI;
    ChassisFollow.PIDChassisFollow.Ad = CHASSIS_FOLLOW_AD;
    ChassisFollow.PIDChassisFollow.Bd = CHASSIS_FOLLOW_BD;
    ChassisFollow.PIDChassisFollow.Cd = CHASSIS_FOLLOW_CD;
    ChassisFollow.PIDChassisFollow.ErrRef = 0.0f;

    ChassisFollow.PIDChassisFollow.clear(&ChassisFollow.PIDChassisFollow);
    ChassisFollow.SetFollowValue = CHASSIS_FOLLOW_INIT_VALUE;

    ChassisSpeedPIDControl.Kp = 0.0001f;
    ChassisSpeedPIDControl.Ki = 0.0f;
    ChassisSpeedPIDControl.Kd = 0.0f;
    ChassisSpeedPIDControl.OutMin = -0.1f;
    ChassisSpeedPIDControl.OutMax = 0.1f;
    ChassisSpeedPIDControl.ErrRef = 0.0f;
    ChassisSpeedPIDControl.calc = &PidCalc;

    //功率环部分
#if infantryID == 0
    Power.R = 0.01f;
#if USE_SEPERATE_POWER == 0 //四个轮使用独立功率环
    Power.kk = 5.7f;
#elif USE_SEPERATE_POWER == 1
    for(int i = 0; i < 4; i++)
        Power.kk[i] = 5.7f;
#endif
    Power.kp_Kincrease = 0.004f;
    Power.K_Power_Buffer = 0.042f;
    Power.upHill_k[0] = 0.5f;
    Power.upHill_k[1] = 0.5f;
    Power.upHill_k[2] = 1.5f;
    Power.upHill_k[3] = 1.5f;
    SpeedLimit.RemoteK = 0.3f;
    SpeedLimit.RemoteIncreaseK = 0.015f;
    SpeedLimit.PowerLimitSpinDefault = 0.25f;

#elif infantryID == 1
    Power.R = 0.1f;
#if USE_SEPERATE_POWER == 0 //四个轮使用独立功率环
    Power.kk = 5.7f;
#elif USE_SEPERATE_POWER == 1
    for(int i = 0; i < 4; i++)
        Power.kk[i] = 5.7f;
#endif
    Power.kp_Kincrease = 0.001f;
    Power.K_Power_Buffer = 0.042f;
    Power.upHill_k[0] = 0.5f;
    Power.upHill_k[1] = 0.5f;
    Power.upHill_k[2] = 1.5f;
    Power.upHill_k[3] = 1.5f;
    SpeedLimit.RemoteK = 0.3f;
    SpeedLimit.RemoteIncreaseK = 0.015f;
    SpeedLimit.PowerLimitSpinDefault = 0.25f;
#endif
}

/*
 * 解算当前平移速度
 *
 * */
float Angle;
void GetWheelMotorStatus()
{
    //云台底盘偏角
    Angle = (CHASSIS_FOLLOW_INIT_VALUE - YawMotor.Location.Location) * 2 * PIE;
    for (int i = 0; i < 4; i++)
    {
        ChassisMotor[i].Speed.Speed = (float)((float)(int16_t)(ChassisMotor[i].CANMessege[2]<<8 | ChassisMotor[i].CANMessege[3]) / 9600.f);
    }

    float chassisDirectSpeedX, chassisDirectSpeedY;
    chassisDirectSpeedX = -0.25f * (ChassisMotor[0].Speed.Speed + ChassisMotor[1].Speed.Speed - ChassisMotor[2].Speed.Speed - ChassisMotor[3].Speed.Speed);
    chassisDirectSpeedY = 0.25f * (ChassisMotor[1].Speed.Speed + ChassisMotor[2].Speed.Speed - ChassisMotor[0].Speed.Speed - ChassisMotor[3].Speed.Speed);
    ChassisSpeed.SpeedY = chassisDirectSpeedY * cosf(Angle) + chassisDirectSpeedX * sinf(Angle);
    ChassisSpeed.SpeedX = -chassisDirectSpeedY * sinf(Angle) + chassisDirectSpeedX * cosf(Angle);

    ChassisSpeed.SpeedX = 0.01f * ChassisSpeed.SpeedX + 0.99f * ChassisSpeed.lastSpeedX;
    ChassisSpeed.SpeedY = 0.01f * ChassisSpeed.SpeedY + 0.99f * ChassisSpeed.lastSpeedY;

    ChassisSpeed.lastSpeedX = ChassisSpeed.SpeedX;
    ChassisSpeed.lastSpeedY = ChassisSpeed.SpeedY;
}

/*
 * 解算Yaw电机数据
 *
 * */
void GetYawMotorStatus()
{
    static float YawEncoderData = CHASSIS_FOLLOW_INIT_VALUE, YawEncoderDataLast = CHASSIS_FOLLOW_INIT_VALUE;
    static int YawEncoderCount = 0;

    //yaw位置数据解算及归一化
    YawEncoderData	=	(short int)((YawMotor.CANMessege[0]<<8)|YawMotor.CANMessege[1]);
    YawEncoderData	=	YawEncoderData	/	MaxEncoder_6020;

    //记圈
    if(YawEncoderData -	YawEncoderDataLast > 0.8f)
        YawEncoderCount--;
    else if(YawEncoderData - YawEncoderDataLast	< -0.8f)
        YawEncoderCount++;

    //yaw电机失联
    if(GimbalReceive.LostCounter)
        YawEncoderCount=0;

    YawMotor.Location.Location = YawEncoderData;
    YawEncoderDataLast = YawEncoderData;
}

/*!
 * @brief 获取当前功率
 * @return  当前功率
 */
float GetPowerNow()
{
    uint16_t ErrorCode = GetErrorState();

    //超级电容测得功率更准，优先使用超级电容测得功率。当超级电容失联时则使用裁判系统数据
    if((ErrorCode >> SUPERC_LOST_COUNT) & 1)//超级电容丢失
        return judge_rece_mesg.power_heat_data.chassis_power;
    if(superCap.power_fb.p_wheel == 0 && superCap.power_fb.p_sc == 0 &&
        (superCap.scFeedback.V_SC > 27.0f || superCap.scFeedback.V_SC <= 0.f)) //电容数据出问题
        return judge_rece_mesg.power_heat_data.chassis_power;

    return SC_get_power_wheel();

}

/*!
 * @brief 获取当前缓冲能量
 * @return 缓冲能量大小
 */
float GetPowerBuffer()
{
    uint16_t ErrorCode = GetErrorState();

    /////////////////ErrorCheck////////////////////
    if((ErrorCode >> JUDGEMENT_LOST_COUNT) & 1)
        return Power.Buffer;

    //使用超级电容时，缓冲能量限制在某个值以上
    if (Status.Use_AutoC)
    {
        uint16_t buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer < 35 ? 35 : judge_rece_mesg.power_heat_data.chassis_power_buffer;
        if(buffer>250) buffer = 60;//避免数据接收错误，限制缓冲能量大小,
        return buffer;
    }
    return judge_rece_mesg.power_heat_data.chassis_power_buffer;
}

float ChangePowerLimit = MinChangePower;
float powerIncrement=0;
extern enum SuperCapState super_cap_state;
/*
 * 获取当前限制功率
 * */
float GetPowerLimit()
{
    uint16_t ErrorCode = GetErrorState();
    float chassis_power_limit = judge_rece_mesg.robot_status_data.chassis_power_limit;
    /* 错误检测 */
    if((ErrorCode >> JUDGEMENT_LOST_COUNT) & 1)///裁判系统丢失
        return judgement_protection.chassis_power_limit;

    if((ErrorCode >> SUPERC_LOST_COUNT) & 1) //超级电容丢失时使用裁判系统数据
        return judge_rece_mesg.robot_status_data.chassis_power_limit;

    //当电容空时，避免超功率，不提升功率大小
    if(super_cap_state==EMPTY)
    {
       return (float)judge_rece_mesg.robot_status_data.chassis_power_limit;
    }

    //使用飞坡超级电容
    if(Status.Use_RampC && (fabsf(GimbalReceive.SetSpeedX) + fabsf(GimbalReceive.SetSpeedY)) > 0.01)
    {
        //线性增加限制功率
        ChangePowerLimit += ChangePowerIncrease;
        if(ChangePowerLimit > MaxChangePower)
            ChangePowerLimit = MaxChangePower;

        //电压低时不允许目标功率过高，否则可能超功率寄
        if(superCap.scFeedback.V_SC<=12)
        {
            ChangePowerLimit = judge_rece_mesg.robot_status_data.chassis_power_limit;
        }
//        else if(superCap.scFeedback.V_SC <= 16)
//        {
//            //超级电容在低于16V时，最大以15A放电，给一定阈值，10A最大放电电流
//            float maxChargePower = superCap.scFeedback.V_SC * 10;
//            if(ChangePowerLimit >= maxChargePower + (float)judge_rece_mesg.robot_status_data.chassis_power_limit)
//            {
//                ChangePowerLimit = maxChargePower + (float)judge_rece_mesg.robot_status_data.chassis_power_limit;
//            }
//        }
        return ChangePowerLimit;
    }

    //其他情况
    else if(Status.Use_RampC)
    {
        ChangePowerLimit = MinChangePower;
        return ChangePowerLimit;
    }


    //使用超级电容，功率上限提高
    if(Status.Use_AutoC)
    {
        //拟合一条随电容电压而变化增加功率的曲线
        powerIncrement = (-0.0003f * powf(superCap.scFeedback.V_SC,4) + 0.4024f * powf(superCap.scFeedback.V_SC,2)-104.46f) * 0.5f;
        if(superCap.scFeedback.V_SC<=19.0f)
        {
            powerIncrement=0;
        }

        if(powerIncrement >= 15.f)
        {
            powerIncrement = 15.f;
        }
        else if(powerIncrement <= 0)
        {
            powerIncrement = 0;
        }
        chassis_power_limit= powerIncrement + (float)judge_rece_mesg.robot_status_data.chassis_power_limit;
    }
    return chassis_power_limit;
}

/*
 * 计圈
 * */
int GetCircleNum(ChassisSpeedMessegePort Chassisspeed)
{
    //这个函数在每个版本的代码中都不一样，无法保证准确性。需要在测试过程中调试。
    static int Circlenum = 0, is_InitFollow = 1;;
    static float FollowNow = 0, FollowLast = 0;

    FollowNow = ChassisFollow.FollowValue;

    if(fabs(Chassisspeed.SetSpeedX) + fabs(Chassisspeed.SetSpeedY) < 0.04)
        is_InitFollow = 1;

    if(is_InitFollow || GimbalReceive.LostCounter){//一旦关控或丢数据
        is_InitFollow = 0;
        if(ChassisFollow.SetFollowValue - FollowNow < -0.5){
            Circlenum = -1;
        }
        else if(ChassisFollow.SetFollowValue - FollowNow > 0.5){
            Circlenum = 1;
        }
        else{
            Circlenum = 0;
        }
    }

    if(FollowNow-FollowLast > 0.6 && (FollowNow + Circlenum - 1) - ChassisFollow.SetFollowValue > -1){
        Circlenum--;
    }
    else if(FollowNow-FollowLast < -0.6 && (FollowNow + Circlenum + 1) - ChassisFollow.SetFollowValue < 1){
        Circlenum++;
    }
    //以上两条无法保证正确性，对于正常情况下来讲，误差绝对值不能超过1.
    FollowLast = FollowNow;

    return Circlenum;
}

/*!
 * @brief 将速度分解至沿着云台方向
 * @param Chassisspeed 底盘速度结构体
 */
static int spinCnt=0;
void ChassisShakeCalculate(ChassisSpeedMessegePort *Chassisspeed)
{
    float CosAngle, SinAngle, SpeedXTemp, SpeedYTemp;
    float NowAngle;

    if(GimbalReceive.SpinFlag)
    {
        //遥控模式下由于存在正反转，偏置不同
        if(!GimbalReceive.keyboardFlag)
        {
#if infantryID==1
            if(spinCnt==0)
            {

                NowAngle = (CHASSIS_FOLLOW_INIT_VALUE - FOLLOW_OFFSET - ChassisFollow.FollowValue) * 2 * PIE;
            }
            else
            {

                NowAngle = (CHASSIS_FOLLOW_INIT_VALUE + FOLLOW_OFFSET2 - ChassisFollow.FollowValue) * 2 * PIE;
            }
#elif infantryID==0
            if(spinCnt==0)
            {

                NowAngle = (CHASSIS_FOLLOW_INIT_VALUE - FOLLOW_OFFSET - ChassisFollow.FollowValue) * 2 * PIE;
            }
            else
            {

                NowAngle = (CHASSIS_FOLLOW_INIT_VALUE + FOLLOW_OFFSET2 - ChassisFollow.FollowValue) * 2 * PIE;
            }
#endif
        }
        //键鼠模式下转向一样
        else
        {
#if infantryID==1
            NowAngle = (CHASSIS_FOLLOW_INIT_VALUE + FOLLOW_OFFSET2 - ChassisFollow.FollowValue) * 2 * PIE;
#elif infantryID==0
            NowAngle = (CHASSIS_FOLLOW_INIT_VALUE + FOLLOW_OFFSET - ChassisFollow.FollowValue) * 2 * PIE;
#endif
        }
    }
    else
    {
        NowAngle = (CHASSIS_FOLLOW_INIT_VALUE - ChassisFollow.FollowValue) * 2 * PIE;
    }
    CosAngle = cosf(NowAngle);
    SinAngle = sinf(NowAngle);

    SpeedXTemp = Chassisspeed->SetSpeedX;
    SpeedYTemp = Chassisspeed->SetSpeedY;

    Chassisspeed ->	SetSpeedY =	(CosAngle * SpeedYTemp - SinAngle * SpeedXTemp);
    Chassisspeed ->	SetSpeedX =	(SinAngle * SpeedYTemp + CosAngle * SpeedXTemp);
}

extern float randomSpin;
float NowpowerLimit;
static int modeFlag=0; //初始化
void ChassisFollowCalculate(ChassisSpeedMessegePort *Chassisspeed)
{
    //在此函数内部进行分工 速度spin分量主要分为三个模式 分别是跟随 取消跟随和陀螺

   ChassisFollow.FollowValue = YawMotor.Location.Location;//当前位置
   Power.Judge_Limit = GetPowerLimit();

    //*******************锁定模式******************//
   if(GimbalReceive.LockFlag)
   {
       modeFlag = 1;
       Chassisspeed->Spin = 0;
       Chassisspeed->SetSpeedX = 0;
       Chassisspeed->SetSpeedY = 0;
   }

   //*********************小陀螺******************//
   else if(GimbalReceive.SpinFlag)
   {
       if(modeFlag == 1)
       {
           //每次开启小陀螺旋转方向不一样
           spinCnt ^= 1;
       }
       modeFlag = 0;

       float sum;
       sum = fabsf(ChassisSpeed.SetSpeedX) + fabsf(ChassisSpeed.SetSpeedY);//平移速度和


       NowpowerLimit = GetPowerLimit();
       float NowSpin;

#if USE_RANDOM_SPIN
       SpeedLimit.PowerLimitSpinDefault = randomSpin;
#else
       SpeedLimit.PowerLimitSpinDefault =0.35f;
#endif


#if infantryID == 0
       //不同限制功率下不同小陀螺转速
       if(NowpowerLimit>=130)
       {
           NowpowerLimit = 130;
       }
       else if(NowpowerLimit<=45)
       {
           NowpowerLimit = 45;
       }
       NowSpin = SpeedLimit.PowerLimitSpinDefault + 0.003f * NowpowerLimit - 0.08f;


       //上坡限制小陀螺速度
       if(GimbalReceive.upHillFlag)
       {
           NowSpin *= 0.4f;
       }

       //平移时降低旋转速度，静止时保证较快转速
       if(sum>=0.1f)
       {
           NowSpin = (1.f - 0.15f*sum)*NowSpin;
       }
       Chassisspeed->Spin = NowSpin;
#elif infantryID == 1
       //不同限制功率下不同小陀螺转速
       if(NowpowerLimit>=110)
       {
           NowpowerLimit = 110;
       }
       else if(NowpowerLimit<=50)
       {
           NowpowerLimit = 50;
       }
       //NowSpin = SpeedLimit.PowerLimitSpinDefault + 0.0045f * NowpowerLimit - 0.15f;
       NowSpin = SpeedLimit.PowerLimitSpinDefault + 0.003f * NowpowerLimit - 0.1f;

       //上坡限制小陀螺速度
       if(GimbalReceive.upHillFlag)
       {
           NowSpin *= 0.4f;
       }

       //平移时降低旋转速度，静止时保证较快转速
       if(sum>=0.1f)
       {
           NowSpin = (1.f - 0.2f*sum)*NowSpin;
       }
       Chassisspeed->Spin = -NowSpin;
#endif


       //计算X Y两方向运动速度占比
       if (fabsf(ChassisSpeed.SetSpeedX) + fabsf(ChassisSpeed.SetSpeedY) > 0.2)
       {
           float k1, k2;
           k1 = ChassisSpeed.SetSpeedX / sum;
           k2 = ChassisSpeed.SetSpeedY / sum;

           //限制平移移动速度，避免小陀螺与平移速度相互制约影响移动效果

            /*
             * 以下是为了平衡小陀螺与平移之间关系
             * 当转速较低时，考虑限制平移速度与转动速度和小于1(电机转速上限)
             * 当转速较高时，给平移限制速度为定值，但可能存在走不了直线问题
             */
            if(fabsf(ChassisSpeed.Spin)<=0.35)
            {
                ChassisSpeed.SetSpeedX = k1 * 0.35f;
                ChassisSpeed.SetSpeedY = k2 * 0.35f;//避免限制陀螺移动速度
            }
            else
            {
                ChassisSpeed.SetSpeedX = k1 * 0.3f;
                ChassisSpeed.SetSpeedY = k2 * 0.3f;//避免限制陀螺移动速度
            }
       }


       static float SpeedX_bias_sum = 0,SpeedY_bias_sum = 0;
       // 小陀螺时由于地不平 可能存在原地漂移情况，为抑制这一问题，利用运动学逆解算，采用类似于反馈思路进行补偿
       if(GimbalReceive.SetSpeedY == 0 && GimbalReceive.SetSpeedX == 0)
       {
            ChassisSpeed.SetSpeedY = - 2.0f * ChassisSpeed.SpeedY;
            ChassisSpeed.SetSpeedX = - 2.0f * ChassisSpeed.SpeedX;
       }

       //遥控模式下每次进小陀螺模式转向不一样
       if(!GimbalReceive.keyboardFlag)
       {
           if(spinCnt==0)
           {
               ChassisSpeed.Spin = - ChassisSpeed.Spin;
           }
       }

   }

   //*******************跟随模式******************//
   else
   {
       modeFlag = 1;
       ChassisFollow.PIDChassisFollow.Ref = CHASSIS_FOLLOW_INIT_VALUE + ChassisFollow.OffsetValue;
       ChassisFollow.PIDChassisFollow.Fdb = ChassisFollow.FollowValue + (float)GetCircleNum(*Chassisspeed);

       float myerr = ChassisFollow.PIDChassisFollow.Ref - ChassisFollow.PIDChassisFollow.Fdb;
       //这个地方是在加入死区之后防止车在死区边缘反复横跳用的，上代测试效果较好故保留
       if (myerr > CHASSIS_NOT_FOLLOW_TOLERANCE)
           ChassisFollow.PIDChassisFollow.Ref -= CHASSIS_NOT_FOLLOW_TOLERANCE;
       else if (myerr < -CHASSIS_NOT_FOLLOW_TOLERANCE)
           ChassisFollow.PIDChassisFollow.Ref += CHASSIS_NOT_FOLLOW_TOLERANCE;
       else
           ChassisFollow.PIDChassisFollow.Ref = ChassisFollow.PIDChassisFollow.Fdb;
       ChassisFollow.PIDChassisFollow.calc(&ChassisFollow.PIDChassisFollow);
#if infantryID == 0
       ChassisSpeed.Spin = -1.0f * ChassisFollow.PIDChassisFollow.Out;
#elif infantryID == 1
       ChassisSpeed.Spin = 1.0f * ChassisFollow.PIDChassisFollow.Out;
#endif
   }

    //***********分工结束 统一运行陀螺模式函数保证无论何时车子均沿云台方向运动******//
    ChassisShakeCalculate(Chassisspeed);
}

/**
 * 返回值表示当前速度与目标速度差距大小
 */
float JudgeChassisMotorError()
{
#if Mecanum_Chassis
    for (int i = 0; i < 4; i++) {
        if (fabs(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) >= 0.3)//如果差距太大了 0.3
            return -2.0f;//如果有一个超过了就返回.数值绝对值表示了升降幅的大小。
    }
    if(fabs(ChassisMotor[0].Speed.SetSpeed-ChassisMotor[0].Speed.Speed) < 0.15
        && fabs(ChassisMotor[1].Speed.SetSpeed-ChassisMotor[1].Speed.Speed) < 0.15
          && fabs(ChassisMotor[2].Speed.SetSpeed-ChassisMotor[2].Speed.Speed) < 0.15
            && fabs(ChassisMotor[3].Speed.SetSpeed-ChassisMotor[3].Speed.Speed) < 0.15)//差距很小了 0.15
        return 1.0f;//全部小于0.15了才返回
#elif Omni_Chassis
    /*
     * 向前加速或向后加速
     * 向前加速时，LastSetSpeedY-SetSpeedY为正，0号电机转速>=0
     * 向后加速时，LastSetSpeedY-SetSpeedY为负，0号电机转速<=0
    */
    if(((ChassisSpeed.LastSetSpeedY-ChassisSpeed.SetSpeedY)*(ChassisMotor[0].Speed.Speed) >=0))
    {
        for (int i = 0; i < 4; i++)
        {
            if (fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) >= 0.15)//如果差距太大了 0.15
                return -1.5f;//如果有一个超过了就返回.数值绝对值表示了升降幅的大小。
        }

        if(fabsf(ChassisMotor[0].Speed.SetSpeed-ChassisMotor[0].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[1].Speed.SetSpeed-ChassisMotor[1].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[2].Speed.SetSpeed-ChassisMotor[2].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[3].Speed.SetSpeed-ChassisMotor[3].Speed.Speed) < 0.05)
            return 2.0f;//全部小于0.05了才返回
    }
    //向前减速,因为前后悬挂差异，考虑向前减速和向后减速用不同参数
    else if((ChassisSpeed.LastSetSpeedY-ChassisSpeed.SetSpeedY<0) && (fabsf(ChassisSpeed.SetSpeedX) <= 0.1))
    {
        for (int i = 0; i < 4; i++)
        {
            if ((fabsf(ChassisMotor[i].Speed.SetSpeed) - fabsf(ChassisMotor[i].Speed.Speed)) >= 0.01)//如果差距太大了 0.3
            {
                return -0.5f;
            }
            else if((fabsf(ChassisMotor[i].Speed.SetSpeed) - fabsf(ChassisMotor[i].Speed.Speed)) <= -0.01)
            {
                return 3.0f;
            }
        }
    }
    //向后减速
    else if((ChassisSpeed.LastSetSpeedY-ChassisSpeed.SetSpeedY>=0) && (fabsf(ChassisSpeed.SetSpeedX) <= 0.1))
    {
        for (int i = 0; i < 4; i++)
        {
            if ((fabsf(ChassisMotor[i].Speed.SetSpeed) - fabsf(ChassisMotor[i].Speed.Speed)) >= 0.02)//如果差距太大了 0.3
            {
                return -0.5f;
            }
            else if((fabsf(ChassisMotor[i].Speed.SetSpeed) - fabsf(ChassisMotor[i].Speed.Speed)) <= -0.02)
            {
                return 0.5f;
            }
        }
    }
    //左右移动时
    else
    {
        for (int i = 0; i < 4; i++)
        {
            if (fabsf(ChassisMotor[i].Speed.SetSpeed - ChassisMotor[i].Speed.Speed) >= 0.2)//如果差距太大了 0.3
                return -2.0f;//如果有一个超过了就返回.数值绝对值表示了升降幅的大小。
        }
        if(fabsf(ChassisMotor[0].Speed.SetSpeed-ChassisMotor[0].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[1].Speed.SetSpeed-ChassisMotor[1].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[2].Speed.SetSpeed-ChassisMotor[2].Speed.Speed) < 0.05
           && fabsf(ChassisMotor[3].Speed.SetSpeed-ChassisMotor[3].Speed.Speed) < 0.05)
            return 1.0f;//全部小于0.05了才返回
    }
#endif
    return 0;
}

/*!
 * @brief 通过限速避免走直线异常问题
 * @param ChassisSpeed 当前速度结构体
 */
 int flag=0;
 float SpeedSum;
void PowerControlBySpeed(ChassisSpeedMessegePort *ChassisSpeed)
{
    Power.Judge_Limit = GetPowerLimit();
    Power.NowPower = GetPowerNow();

    SpeedSum = 1.2f * fabsf(GimbalReceive.SetSpeedX) + 1.2f * fabsf(GimbalReceive.SetSpeedY); //总速度

    if (SpeedSum > 0.01)
    {
        flag=1;
        SpeedLimit.RemoteK += JudgeChassisMotorError() * SpeedLimit.RemoteIncreaseK;
    }
    else
    {
        flag=2;
        SpeedLimit.RemoteK = 0.2f;
    }

//*************限幅*************//
    if (SpeedLimit.RemoteK < 0.1f) SpeedLimit.RemoteK = 0.1f;
    if (SpeedLimit.RemoteK > 1) SpeedLimit.RemoteK = 1;

   if(GimbalReceive.SpinFlag)
   {
       SpeedLimit.RemoteK = 1;
   }

    //这个东西只用于限制平移速度 包括陀螺移动的平移 不然就相当于额外限制了陀螺速度。如果某功率下陀螺移动速度很慢 则说明陀螺速度快了 回去调相应功率下的陀螺速度 不要到这里来找remotek的麻烦
    ChassisSpeed->SetSpeedX = ChassisSpeed->SetSpeedX * SpeedLimit.RemoteK;
    ChassisSpeed->SetSpeedY = ChassisSpeed->SetSpeedY * SpeedLimit.RemoteK;

    //减速时低通滤波，避免速度突变翻车
    if(!GimbalReceive.SpinFlag){
        if(fabsf(ChassisSpeed->LastSetSpeedY) > fabsf(ChassisSpeed->SetSpeedY) || ChassisSpeed->LastSetSpeedY * ChassisSpeed->SetSpeedY < 0)
            ChassisSpeed->SetSpeedY = ChassisSpeed->SetSpeedY * 0.01f + ChassisSpeed->LastSetSpeedY * 0.99f;
    }

    //保存上一次速度
    ChassisSpeed->LastSetSpeedY = ChassisSpeed->SetSpeedY;
    ChassisSpeed->LastSetSpeedX = ChassisSpeed->SetSpeedX;

    ChassisMotor_SpeedUpdate(*ChassisSpeed, 1.0f, 1.0f, 1.0f);
}

void ChassisPowerControl(void)
{
    //************功率输入********************//
    Power.Buffer = GetPowerBuffer();
    Power.Judge_Limit = GetPowerLimit();
    Power.NowPower = GetPowerNow();

    /*
     * 当前底盘功率管控方案
     * 由于底盘功率存在限制（限制功率大小为Power.NowPower)，发送电流不能直接按PID输出发送，而需要乘以系数k（即Power.k)使得电机功率满足限制功率要求
     * 为计算当前限制功率下需要的电流大小，用以下公式计算
     * P_limit= (k * I)^2 * R + (k * I) * v_speed * K_k （1）
     * 其中，P_limit即为当前限制功率(Power.LimitedPower_Now)， I为速度环PID计算结果，k为电流乘的系数(即Power.k)，K_k,R为一参数
     * 方案理解：底盘功率可视为热损耗与输出机械功率两部分，(k * I)^2 * R即为电阻热损耗， (k * I) * v_speed * K_k为输出机械功率
     *         机械功率为T*omega=C_t*phi*I*omega正比于电流I*v_speed(omega为转速rpm,具体转矩T公式参考电机学)，K_k即为固定参数
     *         同样的，R电阻也为固定参数，但R与K_k均未知，只有自己调参（理想情况下R，K_k均接近实际参数，但参数不一定调的准，故采用以下方案）：
     *         将K_k利用负反馈设置为动态参数，当当前功率小于限制功率时，减小K_k值以增大k
     * 调参指南： 主要调整R和kp_Kincrease, R极大影响功率超调程度（R越大，响应阻尼越大，超调越小），kp_Kincrease影响Power.kk的收敛速度，
     *          kp_Kincrease较大可能会引起功率震荡。一般地，我们希望启动时功率较大能更快启动，所以允许存在一定超调，kp_Kincrease较小收敛慢，
     *          较大可能功率震荡严重，需要尝试，同时可以将kk的最终收敛值设为kk初值，保证快速收敛。
     *
     * Power.powerall_ax为（1）式的二次项(k^2)系数和，Power.powerall_bx为（2）式的一次下行系数和，最终求解关于k二次方程得到电流所乘比例系数k
     * */

    //***************解算一个内部使用的功率限制********//

    float K_LimitedPower_Max;
    K_LimitedPower_Max = 1.22f;
    if (GimbalReceive.upHillFlag== StatusOn)
        K_LimitedPower_Max = 1.45f;

    //限制功率参数，该参数正比于缓冲能量大小与当前限制功率大小
    float K_LimitedPower = K_LimitedPower_Max - 1.2f * expf( -0.052f * Power.Buffer);
    if (Power.NowPower > Power.Judge_Limit && Power.Buffer < 15.0f) K_LimitedPower = 0.2f; //马上就要超功率了

    if (Power.Buffer < 10.0f) K_LimitedPower = 0.1f;
    if(K_LimitedPower >= 1.08) K_LimitedPower = 1.08f; //避免限制功率过大，可以给一点启动功率
    else if(K_LimitedPower < 0.1) K_LimitedPower = 0.1f;
    Power.LimitedPower_Now = Power.Judge_Limit * K_LimitedPower;

/////*************速度解算和功率限制*******************///////////


    for (int i = 0; i < 4; i++){
        ChassisMotor[i].PIDSpeed.Ref = ChassisMotor[i].Speed.SetSpeed;
        ChassisMotor[i].PIDSpeed.Fdb = ChassisMotor[i].Speed.Speed;

        if (Status.USECALC2 == StatusOn) ChassisFollow.PIDChassisFollow.calc2(&ChassisMotor[i].PIDSpeed); //变结构PID
        else                                 ChassisFollow.PIDChassisFollow.calc(&ChassisMotor[i].PIDSpeed);

        //加个前馈，影响好像不大？
        if(GimbalReceive.SpinFlag)
        {
            ChassisMotor[i].PIDSpeed.Out += ChassisMotor[i].Speed.SetSpeed* 0.3f;
            if(ChassisMotor[i].PIDSpeed.Out >1)
            {
                ChassisMotor[i].PIDSpeed.Out = 1;
            }
            else if(ChassisMotor[i].PIDSpeed.Out < -1)
            {
                ChassisMotor[i].PIDSpeed.Out = -1;
            }
        }

        //根据速度环PID计算需要发给电调的电流数据大小，但由于存在超功率风险，需要计算这个PID输出需要乘的系数即Power.k
        ChassisMotor[i].precurrent = ChassisMotor[i].PIDSpeed.Out;
        ChassisMotor[i].ax = Power.R * ChassisMotor[i].precurrent * ChassisMotor[i].precurrent;
#if USE_SEPERATE_POWER == 0
        ChassisMotor[i].bx = Power.kk * ChassisMotor[i].Speed.Speed * ChassisMotor[i].precurrent;
#elif USE_SEPERATE_POWER == 1
        ChassisMotor[i].bx = Power.kk[i] * ChassisMotor[i].Speed.Speed * ChassisMotor[i].precurrent;
#endif

        if (ChassisMotor[i].ax <= 0)
            ChassisMotor[i].ax = 0.000001f;
    }
#if USE_SEPERATE_POWER == 0
    Power.powerall_ax = ChassisMotor[0].ax + ChassisMotor[1].ax + ChassisMotor[2].ax + ChassisMotor[3].ax;//二次项系数和
    Power.powerall_bx = ChassisMotor[0].bx + ChassisMotor[1].bx + ChassisMotor[2].bx + ChassisMotor[3].bx;//一次项系数和

    Power.k = (-Power.powerall_bx + sqrtf(Power.powerall_bx * Power.powerall_bx + 4.f * Power.powerall_ax * (Power.LimitedPower_Now) / 80.0f)) /(2.f * Power.powerall_ax); //求根公式
    if (Power.k > 1){
        //如果功率没有超限，k不用起作用
        Power.k = 1;
    }
    else
    {
        if (Power.powerall_ax != 0 && Power.powerall_bx != 0){
            //负反馈调节kk值
            Power.kk = Power.kk + Power.kp_Kincrease * (Power.NowPower - Power.LimitedPower_Now);
        }
        if (Power.kk < 4)       Power.kk = 4;
        else if (Power.kk > 22) Power.kk = 22;
    }
    if (GetPowerBuffer() < 10.0f) Power.k = 0.0f; //防止超功率死掉
#elif USE_SEPERATE_POWER == 1
    for(int i = 0; i < 4 ;i++){
        Power.k[i] = (-ChassisMotor[i].bx + sqrtf(ChassisMotor[i].bx * ChassisMotor[i].bx + 4.0f * ChassisMotor[i].ax * (Power.LimitedPower_Now) / 80.0f)) / (2.0f * ChassisMotor[i].ax);
        if(Power.k[i] > 1)
            Power.k[i] = 1;
        else{
//            if(ChassisMotor[i].ax != 0 && ChassisMotor[i].bx != 0)
//                Power.kk[i] = Power.kk[i] + Power.kp_Kincrease * (Power.NowPower - Power.Judge_Limit);
            if(i==1 || i==4) {
                Power.kk[i] = Power.kk[i] + Power.kp_Kincrease * (Power.NowPower - Power.Judge_Limit);
            }else {
                Power.kk[i] = Power.kk[i] + Power.kp_Kincrease2 * (Power.NowPower - Power.Judge_Limit);
            }

            if(Power.kk[i] < 4)         Power.kk[i] = 4;
            else if(Power.kk[i] > 22)   Power.kk[i] = 22;
        }
        if(Power.Buffer < 10.0f) Power.k[i] = 0.0f;
    }
#endif
}


void MaxSpeedLimit()
{
    float MaxSpeed = fabsf(ChassisMotor[0].Speed.SetSpeed);
    if (fabsf(ChassisMotor[1].Speed.SetSpeed) > MaxSpeed) MaxSpeed = fabsf(ChassisMotor[1].Speed.SetSpeed);
    if (fabsf(ChassisMotor[2].Speed.SetSpeed) > MaxSpeed) MaxSpeed = fabsf(ChassisMotor[2].Speed.SetSpeed);
    if (fabsf(ChassisMotor[3].Speed.SetSpeed) > MaxSpeed) MaxSpeed = fabsf(ChassisMotor[3].Speed.SetSpeed);
    if (MaxSpeed > 1.0) {
        for (int i = 0; i < 4; i++){
            ChassisMotor[i].Speed.SetSpeed = ChassisMotor[i].Speed.SetSpeed / MaxSpeed;
        }
    }
}

void ChassisMotor_SpeedUpdate(ChassisSpeedMessegePort ChassisSpeed, float kx, float ky, float kspin)
{
#if Mecanum_Chassis
    ChassisMotor[0].Speed.SetSpeed =
            +kx * ChassisSpeed.SetSpeedX + ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[1].Speed.SetSpeed =
            +kx * ChassisSpeed.SetSpeedX - ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[2].Speed.SetSpeed =
            -kx * ChassisSpeed.SetSpeedX - ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[3].Speed.SetSpeed =
            -kx * ChassisSpeed.SetSpeedX + ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    MaxSpeedLimit();

    for(int i = 0; i < 4 ; i++){
        int16_t temp = ChassisMotor[i].CANMessege[2] << 8 | ChassisMotor[i].CANMessege[3];
        ChassisMotor[i].Speed.Speed = (float)((float)temp / (float)MaxSpeed_3508);
        setspeed[i+1] = ChassisMotor[i].Speed.SetSpeed;
        speed[i+1] = ChassisMotor[i].Speed.Speed;
        speederr[i+1] = fabs(speed[i] - setspeed[i]);
    }
#elif Omni_Chassis
    ChassisMotor[0].Speed.SetSpeed = -kx * ChassisSpeed.SetSpeedX - ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[1].Speed.SetSpeed = -kx * ChassisSpeed.SetSpeedX + ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[2].Speed.SetSpeed = +kx * ChassisSpeed.SetSpeedX + ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    ChassisMotor[3].Speed.SetSpeed = +kx * ChassisSpeed.SetSpeedX - ky * ChassisSpeed.SetSpeedY - kspin * ChassisSpeed.Spin;
    MaxSpeedLimit();

    for(int i = 0; i < 4 ; i++){
        int16_t temp = (int16_t)(ChassisMotor[i].CANMessege[2] << 8 | ChassisMotor[i].CANMessege[3]);
        ChassisMotor[i].Speed.Speed = (float)((float)temp / (float)MaxSpeed_3508);
        setspeed[i+1] = ChassisMotor[i].Speed.SetSpeed;
        speed[i+1] = ChassisMotor[i].Speed.Speed;
    }

#endif
}

extern RemoteDataPortStruct RemoteDataPort;
void UseRemoteUpdate()
{
    ChassisSpeed.SetSpeedX = RemoteDataPort.ChassisSpeedX;
    ChassisSpeed.SetSpeedY = RemoteDataPort.ChassisSpeedY;
    GimbalReceive.SpinFlag = RemoteDataPort.SpinFlag;
    if(RemoteDataPort.SpinFlag)
        ChassisSpeed.Spin = 0.4f;
    else
        ChassisSpeed.Spin = RemoteDataPort.YawIncrement * 3.0f;
    GimbalReceive.autoSuperFlag = RemoteDataPort.Friction;

}

void ChassisSetSpeed(float SpeedX, float SpeedY)
{
    ChassisSpeed.SetSpeedX = SpeedX;
    ChassisSpeed.SetSpeedY = SpeedY;
}


u8 CAN1SendMessege[8]       = {0};
extern int PowerUpFlag;
void ChassisCurrentSend(void)
{
    u8 CAN1SendMessegeLost[8]   = {0};
#if USE_SEPERATE_POWER == 0
    for (int i = 0;i < 4; i++){
        //上坡时前后轮给不同功率
//        if(i==1)
//        {
//            CAN1SendMessege[i * 2] = ((int16_t)(Power.k * ChassisMotor[i].PIDSpeed.Out * 16384)) >> 8;
//            CAN1SendMessege[i * 2 + 1] = ((int16_t)(Power.k * ChassisMotor[i].PIDSpeed.Out * 16384)) & 0x00ff;
//        }
        CAN1SendMessege[i * 2] = ((int16_t)(Power.k * ChassisMotor[i].PIDSpeed.Out * 16384)) >> 8;
        CAN1SendMessege[i * 2 + 1] = ((int16_t)(Power.k * ChassisMotor[i].PIDSpeed.Out * 16384)) & 0x00ff;
    }
#elif USE_SEPERATE_POWER == 1
    for(int i = 0 ;i < 4 ;i++) {
        if (GimbalReceive.upHillFlag && ChassisSpeed.Spin < 0.05)
            Power.k[i] = Power.k[i] * Power.upHill_k[i];
        CAN1SendMessege[i * 2] = ((int16_t) (Power.k[i] * ChassisMotor[i].PIDSpeed.Out * 16384)) >> 8;
        CAN1SendMessege[i * 2 + 1] = ((int16_t) (Power.k[i] * ChassisMotor[i].PIDSpeed.Out * 16384)) & 0x00ff;
    }
#endif

    if (!GimbalReceive.LostCounter && ENABLE_CAN_SEND && (PowerUpFlag==20))
        CAN1_Send_Msg(CAN1SendMessege,8,0x200);
    else
        CAN1_Send_Msg(CAN1SendMessegeLost,8,0x200);
}